#include <maths/Vector3.h>
#include <math.h>
namespace fort
{
	//-- Compare two vectors
	bool Vector3::Equals(Vector3 vec)
	{
		return (x == vec.x && y == vec.y && z == vec.z);
	}


	//-- returns distance given two vectors
	double Vector3::Distance(Vector3 a, Vector3 b)
	{
		float distX = b.x - a.x;
        float distY = b.y - a.y;
		float distZ = b.z - a.z;
        return sqrt(distX*distX + distY*distY + distZ*distZ);
	}

	//-- Returns length
	float Vector3::Length()
	{
		return (float) sqrt((x*x) + (y*y) + (z*z));
	}

	//-- Sets the values of the vector
	void Vector3::Set(float px, float py, float pz)
	{
		x = px;
		y = py;
		z = pz;
	}

	//-- Normalise the vector
	void Vector3::Normalise()
	{
		//-- sets length to 1
        double length = sqrt(x*x + y*y + z*z); 

        if (length != 0.0) {
            float s = 1.0f / (float)length;
            x = x*s;
            y = y*s;
			z = z*s;
        }
	}
	
	//-- Scales a vector3 by the given scalar
	void Vector3::Scale(double scalar)
	{
		x *= scalar;
		y *= scalar;
		z *= scalar;
	}
	
	//-- subtracts the given vector
	void Vector3::Subtract(Vector3 v)
	{
		x -= v.x;
		y -= v.y;
		z -= v.z;
	}


}